/*
 * communication.h
 *
 *  Created on: Apr 26, 2017
 *      Author: zeroun
 */

#ifndef COMMUNICATION_H_
#define COMMUNICATION_H_


#pragma once

#include <array>
#include <ros/ros.h>
#include <pluginlib/class_loader.h>
#include "augmavconn/augmavconn.h"
//#include <mavconn/interface.h>
#include <set>
#include "communication/communication_plugin.h"
#include <std_msgs/UInt8.h>
#include <std_msgs/Float32.h>
#include "communication/utils.h"
#include "smarteye_common/HParamSetSrv.h"

#include "smarteye_common/UInt16Srv.h"

namespace communication {

/**
 * @brief MAVROS node class
 *
 * This class implement mavros_node
 */
class Communication
{
public:
	Communication();
	~Communication() {};

	void spin();

private:
	ros::NodeHandle mavlink_nh;
        ros::NodeHandle nh;


	ros::Publisher mavlink_pub;
	ros::Subscriber mavlink_sub;
        ros::Subscriber hostHeartbeat_sub;
        ros::Subscriber pos_cog_sub;
        ros::ServiceServer receiveHParamSrv;
        ros::ServiceClient vtolStateClient;

        float pos_cog;
        bool isCogSet;

        uint8_t nodeAlive;
        std::recursive_mutex mutex;

        std::set<uint8_t> mmsgToFCU;
        std::set<uint8_t> mmsgToGCS;

        bool closedFilter;
        augmavconn::AugMavConnInterface::AugPtr station_link;

        int tgt_system_id, tgt_component_id;


	pluginlib::ClassLoader<commplugin::CommPlugin> plugin_loader;
	std::vector<commplugin::CommPlugin::Ptr> loaded_plugins;
        //! station link interface -> router -> plugin callback
	std::array<mavconn::MAVConnInterface::MessageSig, 256> message_route_table;
        //TO DO


        //! station link -> ros
	void mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid);
        //! ros -> station link
	void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg);

	//! message router
	void plugin_route_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid);

	bool is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist);
	void add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist);

        //! station link termination callback
	void terminate_cb();
	//! start mavlink app on USB

        void ReceiveHostHeartb(const std_msgs::UInt8& msg);

        void set_cog(const std_msgs::Float32::ConstPtr& cog);
        bool receiveHParamSetSrv(smarteye_common::HParamSetSrv::Request &req, smarteye_common::HParamSetSrv::Response &res);


};

};	// namespace communication




#endif /* COMMUNICATION_H_ */
